//
// Created by 郝羽 on 2017/12/12.
//

#ifndef RTC_XSWITCH2_OVERUSE_ESTIMATOR_HPP
#define RTC_XSWITCH2_OVERUSE_ESTIMATOR_HPP

#include <cstdint>
#include <cstdio>
#include <deque>
#include <string>

#include "bwe/bwe.hpp"

#include <rapidjson/writer.h>
#include <rapidjson/stringbuffer.h>

#include <errno.h>

namespace bwe {

class OveruseEstimator {
    uint32_t ssrc_;
public:
    OveruseEstimator(uint32_t ssrc) : ssrc_(ssrc) {

    }

    ~OveruseEstimator() {

    }

    std::pair<int, double> Estimate(int64_t now, Usage usage, double ts_delta_ms, int64_t t_delta_ms, int size_delta) {
        const double min_frame_period = findMinFramePeriod(ts_delta_ms);
        const double t_ts_delta = t_delta_ms - ts_delta_ms;
        deltas_++;
        if (deltas_ > max_deltas_) {
            deltas_ = max_deltas_;
        }

        E_[0][0] += process_noise_0;
        E_[1][1] += process_noise_1;

        // if usage conflict with offset, enlarge estimate noise covariance to converge faster
        if ((usage == Usage::OverUsing && offset_ < prev_offset_) || (usage == Usage::UnderUsing && offset_ > prev_offset_)) {
            E_[1][1] += process_noise_1 * 10;
        }

        const double h[2] = {(double)size_delta, 1.0};
        const double Eh[2] = {E_[0][0]*h[0] + E_[0][1]*h[1],
                              E_[1][0]*h[0] + E_[1][1]*h[1]};
        const double residual = t_ts_delta - slope_*h[0] - offset_;
        if (usage == Usage::Normal) {
            const double max_residual = 3.0 * std::sqrt(var_noise_);
            if (std::fabs(residual) < max_residual) {
                updateNoiseEstimate(residual, min_frame_period);
            } else {
                updateNoiseEstimate(residual < 0 ? -max_residual : max_residual, min_frame_period);
            }
        }
        const double denom = var_noise_ + h[0]*Eh[0] + h[1]*Eh[1];
        const double K[2] = {Eh[0]/denom,
                             Eh[1]/denom};
        const double IKh[2][2] = {{1.0 - K[0]*h[0],     - K[0]*h[1]},
                                  {    - K[1]*h[0], 1.0 - K[1]*h[1]}};
        const double e00 = E_[0][0];
        const double e01 = E_[0][1];

        // Update state.
        E_[0][0] = e00 * IKh[0][0] + E_[1][0] * IKh[0][1];
        E_[0][1] = e01 * IKh[0][0] + E_[1][1] * IKh[0][1];
        E_[1][0] = e00 * IKh[1][0] + E_[1][0] * IKh[1][1];
        E_[1][1] = e01 * IKh[1][0] + E_[1][1] * IKh[1][1];

        slope_ = slope_ + K[0] * residual;
        prev_offset_ = offset_;
        offset_ = offset_ + K[1] * residual;
        return std::make_pair(deltas_, normalizeOffset());
    };

    double VarNoise() const {
        return var_noise_;
    };

    double Offset() const {
        return offset_ * std::min(60, deltas_);
    };

private:

    double findMinFramePeriod(double ts_delta) {
        double min = ts_delta;
        while (ts_delta_hist_.size() >= min_frame_hist_len_) {
            ts_delta_hist_.pop_front();
        }
        for (const double old : ts_delta_hist_) {
            min = std::min(old, ts_delta);
        }
        ts_delta_hist_.push_back(ts_delta);
        return min;
    };

    void updateNoiseEstimate(double residual, double ts_delta) {
        double alpha = 0.01;
        if (deltas_ > 10*30) {
            alpha = 0.002;
            //alpha = 0.001;
        }
        const double beta = std::pow(1-alpha, ts_delta * 30 / 1000.0);
        avg_noise_ = beta * avg_noise_ + (1-beta) * residual;
        var_noise_ = beta * var_noise_ + (1-beta) * (avg_noise_ - residual) * (avg_noise_ - residual);
        if (var_noise_ < 1) {
            var_noise_ = 1;
        }
    };

    double normalizeOffset() {
        int deltas = std::min(min_deltas_, deltas_);
        return offset_ * deltas;
    };

private:
    const double process_noise_0     = 1e-13;
    const double process_noise_1     = 1e-3;
    const uint32_t    min_frame_hist_len_ = 60;
    const int    min_deltas_         = 60;
    const int    max_deltas_         = 1000;

    int         deltas_                 = 0;
    double      E_[2][2]                = {{100,0},{0,1e-1}};
    double      slope_                  = 8.0 / 512.0;
    double      offset_                 = 0;
    double      prev_offset_            = 0;
    double      avg_noise_              = 0.0;
    double      var_noise_              = 50.0;

    std::deque<double> ts_delta_hist_;
};

}

#endif //RTC_XSWITCH2_OVERUSE_ESTIMATOR_HPP
